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Abstract 

This  paper  presents  a  heuristic  fuzzy  position  estimation  technique  for  autonomous  un¬ 
derwater  vehicle  navigation.  The  heuristic  estimator  performs  asynchronous  data  fusion 
of  all  sensor  measurements  based  on  their  relative  confidence  levels,  and  then  nonlinearly 
combines  the  fused  information  with  the  INS  estimates  via  fuzzy  filtering  techniques.  In 
this  paper,  the  basis  and  implementation  of  the  estimator  will  be  described,  and  naviga¬ 
tion  results  will  be  presented  based  on  the  heuristic  estimator.  In  addition,  performance 
comparison  based  on  the  heuristic  estimator  and  those  based  on  extended  Kalman  filters 
will  be  reported  in  our  companion  paper,  and  the  results  are  expected  to  provide  insights 
into  the  pros  and  cons  of  individual  methods  in  terms  of  computational  cost,  steady-state 
and  convergence  characteristics  for  bias  estimation. 


1  Introduction 


With  rapid  progress  in  COTS  sensors  and  electronics 
technology,  miniaturized  autonomous  underwater  ve¬ 
hicles  (AUV)  have  reached  an  acceptable  level  of  ma¬ 
turity  and  reliability  which  can  be  capitalized  on  their 
use  for  oceanographic  and  military  applications  [5,  6]. 
Examples  include  spatio-temporal  surveys  during  clan¬ 
destine  oceanographic  and  mine  counter-measure  op¬ 
erations  in  shallow- water  environments.  Without  re¬ 
quiring  any  tethering  support,  the  dynamic  stability 
and  data  sampling  quality  can  be  much  improved.  In 
addition,  multiple  small  AUVs  can  be  deployed  simul¬ 
taneously  to  traverse  in  different  regions  without  ne¬ 
cessitating  one-to-one  surface  ships,  and  this  results  in 
higher  data  sampling  efficiency  [7].  To  truly  charac¬ 
terize  four-dimensonal  ocean  dynamics  autonomously, 
high-precision  underwater  navigation  is  a  technically 
challenging  issue  because  vehicle  localization  must  be 
done  onboard.  While  differential  GPS  sensor  technol¬ 
ogy  provides  a  good  solution  for  surface  navigation 
[2],  underwater  navigation  still  remains  a  challenging 
problem  especially  when  sonar  position  beacons  are 
unavailable.  The  main  goal  of  this  paper  is  to  present  a 
novel  heuristic  data  fusion  which  collates  different  and 
independent  asynchronous  position  sensors  together, 
and  nonlinearly  gain  schedules  them  with  the  onboard 
INS  system.  One  important  objective  of  our  study 
is  to  evaluate  the  effectiveness  of  the  algorithmic  per¬ 
formance  based  on  computationally-intensive  model- 
based  approach  [3,  8]  versus  heuristic  sensor-based  ap¬ 


proach  with  respect  to  steady-state  and  convergence 
performance  of  bias  estimation.  This  evaluation  will 
hq  reported  in  our  companion  paper.  This  paper  is 
organized  as  follows:  the  next  section  briefly  describes 
the  sensor  and  capability  of  the  Ocean  Explorer  AUV. 
Section  3  presents  the  heuristic  navigation  architecture 
and  algorithms  currently  implemented  on  the  OEX. 
Section  4  presents  navigation  results  based  on  GPS- 
doppler  aided  INS  system,  and  finally  the  last  section 
provides  concluding  remarks. 


2  Ocean  Explorer 

The  OEX  AUV  is  a  small  and  untethered  autonomous 
underwater  vehicle.  It  is  7  ft  long  (extendable  to  10 
ft)  and  maximum  diameter  is  21".  The  hull  is  based 
on  a  modified  Gertler’s  Series  58  Model  4154  with 
a  modular  mid-section  for  interfacing  swappable  pay- 
loads.  The  cruciform  control  surfaces  are  aft-mounted, 
and  are  replaceable  with  larger  fins  if  a  3'  mid-section 
is  inserted.  In  air.  the  vehicle  weighs  approximately 
4001bs,  and  is  designed  to  be  neutrally  buoyant.  The 
OEX  cruises  at  a  speed  of  3  knots  (a  range  of  2-5 
knots),  and  can  execute  any  pre-programmed  mission 
continuously  for  approximately  12  hours  before  Ni-Cacl 
batteries  are  to  be  re-charged.  Note  that  the  vehi¬ 
cle  mission  can  be  re-programmed  while  the  OEX  is 
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in  water,  thereby  increasing  the  operational  efficiency. 
In  terms  of  navigational  capability,  the  OEX  houses 
1)  an  acoustic  floppier  sensor  which  measures  altitude 
and  vehicle  velocity  with  respect  to  either  water  col¬ 
umn  or  ground;  2)  a  Watson  AHRS  unit  which  mea¬ 
sures  Euler  angles,  tri-axial  body  rates  and  accelera¬ 
tion:  and  3)  a  differential  GPS  receiver  unit;  4)  RF 
ethernet  and  5)  CTD  sensor  instrument  which  pro¬ 
vides  depth  measurement.  All  sensors  and  actuators 
onboard  were  each  attached  to  their  individual  Neuron 
Modules  and  the  low-level  control  communication  is 
done  using  the  LonTalk  protocol.  High-level  planning 
and  control  processes  are  carried  out  on  the  VxWorks 
real-time  operating  system  (with  68030  CPU)  and  the 
communication  among  processes  is  data-driven  using  a 
shared  memory  archiecture  with  semaphores  and  ded¬ 
icated  I/O  function  calls  (see  [5.  6,  7]  for  more  detail). 


3  Heuristic  Navigation  Archi¬ 
tecture 


Figure  1  shows  a  general  architecture  for  setting  up  the 
navigational  module  in  the  OEX.  In  this  architecture, 
there  are  arbiters  created  for  position  sensors,  attitude 
sensors  and  motion  sensors.  This  set  up  is  desirable 
because  many  existing  AUVs  incorporate  multiple  sen¬ 
sors  performing  same  functions,  and  it  is  thus  benefi¬ 
cial  to  fuse  all  information  to  obtain  the  best  naviga¬ 
tion  estimates.  In  cases  of  sensor  failure,  these  arbiters 
will  reconfigure  in  order  to  complete  time- critical  mis¬ 
sions. 

Inputs  to  the  position  sensor  arbiter  are  absolute 
AUV  position  measurements,  which  can  be  based  on 
(D)GPS  and  various  forms  of  baseline  sonar  fixes. 
These  measurements  are  particularly  valuable  because 
of  their  drift-free  properties  over  a  longer  time  scale, 
as  compared  to  the  dead-reckoning  position  estimate. 
However,  over  a  shorter  time  scale,  the  DGPS  fixes 
might  be  correlated  thus  introducing  undesirable  po¬ 
sition  error  for  compensation.  It  is  thus  important  to 
carefully  sample  these  fixes  so  that  the  signal- to-noise 
ratio  is  maximized.  In  addition,  measurements  from 
all  sensors  are  generally  unavailable  at  every  time  sam¬ 
pling  instant,  and  a  strategy  is  thus  needed  to  combine 
these  asynchronous  measurements  before  routing  the 
result  to  the  heuristic  position  estimator. 

To  combine  the  position  measurements,  each  sensor 
is  assigned  a  confidence  value  which  characterizes  its 
expected  variance  in  error  about  the  true  value.  For 
an  example,  typical  DGPS  fixes  can  have  l-5m  range 
error  (a)  together  with  horizontal  dillution  of  preci¬ 
sion  (HD OP)  uncertainty  due  to  satellite  geometry. 
The  total  position  error  introduced  in  terms  of  root- 
mean-square  value  is  HDOP  x  <t.  It  should  be  noted 
that  while  the  range  error  is  generally  hard  to  quan¬ 
tify.  the  HDOP  profile  can  be  readily  obtained  from 
any  receiver,  and  thus  it  should  be  accounted  for  in 
the  position  estimator  otherwise  position  error  might 
be  compromised.  Once  a  suitable  time  sampling  inter¬ 
val  is  chosen,  the  output  of  the  position  sensor  arbiter 
is  given  as 

X  =  J2Zi'Ci  (V) 


where  X  is  the  arbiter  output.  Z ;  is  the  ith  sensor 
output,  and  G;  is  its  confidence  value  which  has  ac¬ 
counted  for  both  the  range  error  and  baseline  geometry 
of  satellites  or  sonar  beacons.  Note  that  a  constraint  of 
y/.-j  C'i  =1  is  imposed  on  the  confidence  values,  and 
thus  C ;  can  be  loosely  interpreted  as  prior  probability 
that  the  ith  sensor  is  correct.  Among  these  position 
sensors  only  the  horizontal  profiles  are  sought  because 
of  their  much  improved  range  error  tolerances  and  the 
fact  that  the  depth  information  can  be  obtained  di¬ 
rectly  from  the  doppler  sonar.  Now  the  remaining  task 
is  to  determine  these  C;. 


One  important  objective  in  this  paper  is  to  strive  for 
a  practical  estimation  algorithm  which  is  not  compu¬ 
tational  intensive  but  yet  provides  theoretically  sound 
approach  in  performing  data  fusion.  To  have  a  rea¬ 
sonable  trade-off,  consider  the  following  condition  in 
which  the  unknown  quantity  to  be  estimated  is  a  con¬ 
stant,  X* ,  and  is  being  observed  by  N  independent 
sensors.  It  can  be  easily  shown  [1]  that,  in  such  a 
case,  an  expression  of  the  minimum  mean-squared  er¬ 
ror  estimate  and  the  resulting  error  variance  can  be 
established 
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where  e;  is  the  expected  position  error  standard  de¬ 
viation  for  the  ith  sensor,  a;  and  hi  represent  the 
range  error  and  HDOP  profile  for  the  ith  sensor,  n  is 
the  product  operator,  and  P  corresponds  to  a  sum  of 
N  unique  combinations  of  variance  terms  where  each 
term  consists  of  a  product  of  N  —  1  variances.  C  cor¬ 
responds  to  the  overall  error  variance.  For  illustrative 
purposes,  consider  N  —3, 
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It  should  be  noted  that  these  confidence  values  are  only 
meaningful  if  the  statistical  properties  of  the  sensor 
measurements  are  stationary,  and  the  state  dynamics 
are  negligible  although  the  effect  of  state  dynamics  will 
be  studied  and  reported  in  our  companion  paper. 


Similarly,  the  attitude  sensor  arbiter  generates  the 
best  estimate  of  the  Euler  angles  and  body-fixed  angu¬ 
lar  rates.  This  arbiter  setting  enables  direct  compari¬ 
son  in  performances  using  different  compasses  without 
requiring  any  code  modification.  The  arbiter  outputs 
are  then  routed  to  both  the  heuristic  position  estima¬ 
tor  and  motion  sensor  arbiter. 


In  the  motion  sensor  arbiter,  the  doppler-based 
ground  and  water  velocities  are  compensated  based 
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on  the  CTD  output.  At  present,  the  default  sound 
speed  was  set  to  1500m/s  and  correction  was  made 
in  post-processing.  Typical  variation  in  sound  speed 
with  respect  to  the  default  setting  can  he  as  high  as 
2%,  and  this  error  must  he  accounted  for  during  long 
underwater  transits. 


3.1  Heuristic  Position  Estimator 
(HPE) 

Figure  2  shows  a  block  diagram  of  the  heuristic  es¬ 
timator  which  considers  only  the  ground  speed  as  a 
special  case  in  order  to  simplify  explanation.  Xn  and 
Xf  correspond  to  the  absolute  position  measurements 
defined  in  north-east  co-ordinates  given  a  pre-defined 
origin.  Similarly.  Xn  and  X£  correspond  to  the  posi¬ 
tion  estimator  outputs  using  the  same  origin.  In  the 
diagram,  the  outputs  of  the  attitude  sensor  arbiter  are 
used  to  transform  the  ground  speed  from  the  body- 
fixed  frame  to  geographical  frame.  If  there  is  no  po¬ 
sition  measurement,  the  position  estimator  is  then  re¬ 
duced  to  a  dead-reckoner,  and  no  offsets  are  needed  for 
Xn  and  Xc.  If  there  exists  at  least  one  position  update, 
the  position  residual  error  (the  difference  between  the 
measurement  and  heuristic  position  estimator  output) 
is  then  nonlinearly  gain  scheduled  in  order  to  gener¬ 
ate  appropriate  offsets  for  Xn  and  X£  for  each  of  the 
absolute  position  measurement,  and  the  nonlinear  Ke 
and  K„  are  given  as 
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From  these  expressions,  it  can  be  easily  seen  that  when 
there  is  small  residual  error  with  respect  to  C.  the 
HPE  discounts  the  contribution  from  the  measurement 
as  primarily  sensor  uncertainty,  and  resorts  to  dead¬ 
reckoning  which  generally  provides  a  smoother  posi¬ 
tion  estimate.  In  the  case  the  residual  error  becomes 
relatively  significant,  the  HPE  assumes  that  the  er¬ 
ror  is  largely  caused  by  the  motion  sensor  drift,  and 
thus  weighs  heavily  on  the  measurement.  Figure  3 
provides  a  flow  chart  of  the  detailed  operation  at  ev¬ 
ery  navigation  cycle.  By  examining  the  flow  chart,  one 
can  see  that,  when  both  water  and  ground  speed  are 
available,  currents  will  be  estimated  in  the  geograph¬ 
ical  frame  after  transforming  the  speed  difference.  In 
cases  where  there  is  a  loss  of  bottom  track  at  large  al¬ 
titude.  the  current  estimate  can  be  used  together  with 
the  water  speed  to  estimate  the  ground  speed  over  a 
relatively  short  time  scale  within  which  the  estimated 
currents  magnitude  is  slowly  decaying.  Two  important 
observations  can  be  made  regarding  the  HPE  perfor¬ 
mance.  Firstly,  the  HPE  does  not  explicitly  estimate 
the  sensor  biases  and  thus  the  navigation  performance 
without  any  position  measurements  is  no  better  than 
that  of  standard  dead-reckoning  estimator.  Secondly, 
the  nonlinear  error  correcting  gains  are  heuristically 


defined  based  on  the  assumption  of  gaussian  sensor 
noise  distribution.  It  should  be  noted  that  in  a  com¬ 
panion  paper,  the  same  data  sets  were  filtered  using 
extended  2D  Kalman  filters  h  and  the  objective  for 
such  arrangement  is  to  compare  the  performance  be¬ 
tween  using  numerically  intensive  algorithms  with  bias 
estimation  and  the  heuristic  algorithms  without  bias 
estimation  but  relying  on  position  measurements.  The 
critical  factors  to  be  considered  for  the  comparison  in¬ 
clude  the  convergence  rate  and  steady  state  error  in 
the  bias  estimates  and  magnitude  of  drift  induced  be¬ 
tween  fixes.  Among  these  data  sets,  only  DGPS  po¬ 
sition  fixes  were  available,  and  thus  the  true  heading 
reference  can  only  be  deduced  when  the  OEX  was  on 
surface.  It  should  be  noted  that,  to  accomodate  asyn¬ 
chronous  data  fusion,  the  position  estimator  integra¬ 
tion  time  step  was  chosen  to  be  the  smallest  inter¬ 
val  with  respect  to  all  sensor  throughputs  (among  the 
results  presented,  the  sampling  rate  was  set  to  8Hz, 
(Tdgps  =2  and  <rgps  —  30.  In  cases  when  there  is  no 
position  fix(es)  available  during  some  time  interval,  no 
offsets  will  be  added  to  the  position  estimator  outputs. 


4  Results 

Navigation  results  presented  in  this  paper  were  based 
on  a  combination  of  the  Motorola  GPS  receiver,  Acu¬ 
point  differential  receiver  and  the  doppler-aided  INS 
system.  To  provide  important  insights,  the  GPS  per¬ 
formance  was  first  characterized  in  a  controlled  envi¬ 
ronment.  followed  by  the  evaluation  of  the  heuristic 
position  estimation  performance  which  was  based  on 
at-sea  data. 

Figure  4  and  5  show  respectively  the  time  histories 
of  fixes  and  the  converted  X-Y  excursion  (at  a  lat¬ 
itude  of  20  degrees.  Boca  Raton,  Florida)  collected 
in  a  single  experiment  during  which  the  OEX  was  lo¬ 
cated  in  a  circular  salt-water  pool  of  10ft  diameter,  and 
the  GPS  and  FM  antennas  were  held  above  the  sur¬ 
face.  It  should  be  noted  that  the  OEX  was  not  rigidly 
mounted  thus  slight  vehicle  movement  should  be  ex¬ 
pected  although  it  was  constrained  to  within  2  ft  in 
radius,  which  is  well  within  the  rated  DGPS  accuracy. 
The  choice  of  pool  test  was  made  so  as  to  minimize 
any  effect  associated  with  the  controlled  environment. 

Time  history  plots  consist  of  longitude  (meter),  lat¬ 
itude  (meter)  and  the  GPS  status  (0:  NO  GPS.  1: 
GPS,  2:  DGPS).  It  can  be  seen  that  the  distribution 
of  GPS  and  DGPS  fixes  were  fairly  uniform,  and  dur¬ 
ing  the  time  when  DGPS  fixes  were  available  (around 
2500sec),  there  was  minimal  fluctuation  in  both  lati¬ 
tude  and  longitude.  On  the  contrary,  when  only  the 
GPS  fixes  were  available  (around  250secj,  there  was 
significant  fluctuation  observed,  and  the  apparent  ve¬ 
locity  speed  was  as  high  as  0.1  knot,  as  indicated  by 
the  time  histories  data.  It  is  thus  important  to  con¬ 
sider  the  effect  of  GPS  correlated  noise  and  sampling 
rate  when  estimating  currents  over  a  short  temporal 
scale.  Nevertheless,  the  navigation  performance  can 
be  also  critically  dependent  on  the  specific  trial  loca- 


1 1t  is  worth  mentioning  that  standard  Kalman  filters  can- 
not  be  directled  applied  to  navigation  problems  because  of  the 
kinematics  transform  between  co-ordinate  frames. 
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Hon,  and  in  particular,  the  effect  of  GPS  noise  will 
become  relatively  insignificant  when  the  OEX  is  to  he 
operated  in  Florida  currents  of  3-5  knots,  and  in  deep 
water  regions  with  only  water  speed  measurements. 

From  the  XY  plot,  it  can  be  seen  that  all  errors  were 
confined  to  within  a  5  meter  square  area.  To  evaluate 
the  worst  case  sampling  interval,  one  can  consider  the 
following  ideal  condition  under  which  the  OEX  cruises 
horizontally  along  for  T  seconds  at  u  knots  with  Su 
speed  error  and  <*)(/>  heading  error.  The  position  error, 
SX.  can  be  crudely  approximated  as 

SX  =  Ts/u2fr<l’2  +  bu 2  (15) 

For  an  example,  given  DGPS  fixes  with  5  meter  er¬ 
ror.  the  OEX  cruises  at  2  knots  with  1%  speed  error 
and  2  degree  heading  error,  T  is  approximately  3  min¬ 
utes.  That  means,  the  maximum  sampling  interval  is 
3  minutes  or  position  estimation  performance  will  be 
significantly  compromised.  On  the  contrary,  T  should 
not  be  much  smaller  than  3  minutes  otherwise  the  es¬ 
timator  will  be  significantly  biased  with  GPS  noise. 

Figure  G  and  7  compare  the  INS  way-point  naviga¬ 
tion  performance  based  on  1)  floppier  returns  (dash 
line)  and  2)  GPS  +  doppler  (solid  line).  In  these 
figures,  'X1  and  ‘O’  represent  a  differential  and  reg¬ 
ular  GPS  fix  respectively,  and  in  both  missions  the 
OEX  was  started  at  the  origin.  During  these  missions, 
the  OEX  was  underwater  most  of  the  time,  and  com¬ 
manded  to  surface  during  some  specified  cornerings  in 
order  to  obtain  fixes,  and  thus  position  drift  due  to 
doppler  and  attitude  sensors  can  be  easily  observed, 
as  compared  to  the  DGPS  fixes  as  the  only  source  of 
reference.  At  the  end  of  the  last  eastward  leg  in  Fig¬ 
ure  6,  there  was  a  significant  discrepancy  between  the 
position  estimator  and  DGPS  measurement  (approxi¬ 
mately  1.5%  error  based  on  50  meters  after  3300  me¬ 
ters  transect).  It  is  worth  mentioning  that  during  pre¬ 
vious  OEX  surface  trials  with  continuous  DGPS  fixes 
(not  included  in  this  paper),  the  doppler  measurement 
error  was  found  to  lie  within  1%.  and  the  additional 
error  herein  was  believed  to  be  caused  mainly  by  the 
heading  measurement  with  internal  magnetic  interfer¬ 
ence.  Figure  7  presents  results  of  a  3-lir  mission  cov¬ 
ering  15km  transect.  Sporadic  fixes  can  be  seen  in  the 
figure  which  corresponds  to  the  OEX  surfacing  maneu¬ 
vers.  Among  these  fixes,  maximum  discrepancy  be¬ 
tween  the  position  estimator  and  GPS  fixes  was  found 
at  location  [-50  east.  150  north],  and  also  it  can  be 
seen  that  the  position  estimator  responds  less  to  these 
GPS  fixes,  but  much  more  to  the  DGPS  fixes  obtained 
immediately  afterwards.  By  observation,  the  discrep¬ 
ancy  was  approximately  100m  since  the  last  update 
(6  legs  of  transect  away  £=  3300m),  and  thus  the  error 
was  approximately  3%.  It  should  be  noted  that  100m 
is  within  the  limit  of  the  GPS  error  deviation,  and  the 
result  suggests  that  accurate  navigation  does  not  nec¬ 
essarily  require  frequent  surfacing,  as  expected  from 
previous  discussion. 


5  Remarks 

In  this  paper,  the  basis  and  implementation  of  the 
heuristic  position  estimator  has  been  described,  and 


navigation  results  which  were  based  on  this  estima¬ 
tor  has  been  illustrated.  This  paper  was  also  empha¬ 
sized  that  the  navigation  performance  must  be  eval¬ 
uated  with  respect  to  the  entire  system  as  a  whole, 
rather  than  an  isolated  entity.  Thus,  factors  such  as 
mission  requirements,  systems  reconfigurability,  cost- 
effectiveness  and  computing  resources  must  be  alto¬ 
gether  considered  with  regard  to  the  constrainted  re¬ 
sources  currently  available  on  the  OEX.  Results  of 
such  evaluation  can  then  provide  important  insights 
into  the  effectiveness  of  sensor-based  and  model-based 
approach.  For  long  underwater  missions  without  any 
sonar  positional  beacons  and  with  minimal  surfacing 
frequency,  estimation  of  sensor  biases  might  be  re¬ 
quired  for  high  precision  navigation,  and  such  infor¬ 
mation  must  be  extracted.  Extended  Kalman  filters 
were  applied  to  those  data  sets  with  known  heading 
bias,  and  the  results  will  be  reported  in  our  compan¬ 
ion  paper  in  terms  of  steady-state  error  and  conver¬ 
gence  performance.  Other  than  focusing  on  the  posi¬ 
tion  sensor  measurements,  parallel  work  on  improving 
our  existing  INS  system  in  currently  underway  such 
as  localizing  the  main  source  of  interference,  demag¬ 
netizing  the  battery  housing,  and  developing  a  more 
accurate  deviation  table  based  on  a  fiber-optic  gyro 
(Andrews  Corp)  as  short-term  heading  reference. 
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Figure  1:  High-level  arbiters  for  setting  up  heuristic 
position  estimator. 


Figure  3:  A  flow  chart  of  the  position  estimator  algo¬ 
rithm. 


Figure  2:  Closed-loop  heuristic  position  estimator. 
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Figure  4:  Motorola  GPS  time- history  error  evaluation. 
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Figure  5:  Motorola  GPS  X-Y  error  evaluation. 
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Figure  7:  Heuristic  position  estimator  and  GPS  out¬ 
puts.  Solid  line:  heuristic  position  estimator  output, 
and  dasli  line:  dead-reckoned  output.  'X'  and  :0:  rep¬ 
resent  differential  and  regular  GPS  fix  respectively. 


Figure  G:  Heuristic  position  estimator  and  GPS  out¬ 
puts.  Solid  line:  heuristic  position  estimator  output, 
and  dash  line:  dead-reckoned  output.  'X1  and  'O'  rep¬ 
resent  differential  and  regular  GPS  fix  respectively. 
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